#include "featurematcher.h"

#include <stdio.h>
#include <iostream>

#include <QMessageBox.h> // for debug only
#include <QString.h> // for debug only

using namespace std;

/******************************************************/
FeatureMatcher::FeatureMatcher()
{}

/******************************************************/
FeatureMatcher::~FeatureMatcher()
{}

/******************************************************/
void FeatureMatcher::match(
	const std::vector<cv::KeyPoint>& key_points1, 
	const std::vector<cv::KeyPoint>& key_points2, 
	const cv::Mat& descriptors1,
	const cv::Mat& descriptors2,
	std::vector<std::pair<int,int> >& correspondances)
{
	// Compute distances between all keypoints
	cv::Mat dists(key_points1.size(), key_points2.size(), CV_64FC1);
	for (unsigned int i=0; i < key_points1.size(); ++i)
	{
		for (unsigned int j=0; j < key_points2.size(); ++j)
		{
			dists.at<double>(i,j) = distance(descriptors1.row(i), descriptors2.row(j));
		}
	}

	// Determine matches exhaustively
	for (unsigned int i=0; i < key_points1.size(); ++i)
	{
		cv::Mat row_matches = dists.row(i);

		// Compute first minimum
		double min, max;
		cv::Point min_pt, max_pt;
		cv::minMaxLoc(row_matches, &min, &max, &min_pt, &max_pt);

		// Mask out first minimum
		cv::Mat mask = cv::Mat::ones(dists.row(i).size(), CV_8UC1);
		mask.at<unsigned char>(min_pt) = 0;

		// Compute second minimum
		double min_2nd, max_2nd;
		cv::Point min_pt_2nd, max_pt_2nd;
		cv::minMaxLoc(row_matches, &min_2nd, &max_2nd, &min_pt_2nd, &max_pt_2nd, mask);

		// Determine if min is worth keeping
		double const threshold_ratio = 0.8;
		if (min/min_2nd < threshold_ratio)
		{
			correspondances.push_back(std::make_pair<int,int>(i, min_pt.x));
		}
		
	}
}

/******************************************************/
double FeatureMatcher::distance(const cv::Mat& attributes1, const cv::Mat& attributes2)
{
	assert(attributes1.size() == attributes2.size());

	const int dim = attributes1.cols; // should be 128 for standard SIFT implementation

	double sum = 0.0;
	for (int i=0; i < dim; ++i)
	{
		const double a = attributes1.at<float>(0,i);
		const double b = attributes2.at<float>(0,i);
		sum += (a-b)*(a-b);
	}
	return sqrt(sum);
}
